clear all;clc
close all;
load('aerodata.mat');
if ~isstruct(pp) || ~isfield(pp, 'form')
    error('pp 不是有效的样条插值结构！');
end
global watch_state;
%[breaks, coefs] = unmkpp(pp); % 尝试提取数据
%pp = mkpp(breaks, coefs);     % 重新创建 pp 结构体

% 仿真参数
x0 = [0,0,0,15*cosd(30),15*sind(30),0.3];
%stop_cond = @(t, y) y(2,1) < 0.5 && t>1;
%options = odeset('Events',@stop_cond);
stop_cond = @(t, y) y(1,1)>PN3_Constant.target_x;%t>3%y(2,1) < 0.5 && t>1;
[t,y]=rk4_fixed_step_stop(@(t,x) dof3PNode(t,x,pp),[0 3],x0,0.01,stop_cond);%,options);
t_ = t.';
y_ = y.';
% function [value, isterminal, direction] = stop_cond(t, y)
%     value = y(2) - 0.5;  % y(2) 表示高度，触地条件 y(2) == 0.5
%     isterminal = 1;  % 终止仿真
%     direction = -1;  % 只检测高度从上往下（正到负）穿过 0.5
% end


figure(1);
plot3(y_(1,:),-y_(3,:),y_(2,:),'r');
grid on;
xlabel('x');
ylabel('y');
zlabel('z');
hold on;
scatter3(PN3_Constant.target_x,-PN3_Constant.target_z,PN3_Constant.target_y,'filled');

% 过载指令
figure(2);
plot(watch_state(1,:),watch_state(2,:),'r');
hold on;
xlabel('t');
ylabel('ax(m/s^2)');
title('x轴过载指令');
grid on;

figure(3);
plot(watch_state(1,:),watch_state(3,:),'r');
hold on;
xlabel('t');
ylabel('ay(m/s^2)');
title('y轴过载指令');
grid on;

figure(4);
plot(watch_state(1,:),watch_state(4,:),'r');
hold on;
xlabel('t');
ylabel('az(m/s^2)');
title('z轴过载指令');
grid on;